#ifndef VIOBOT_H
#define VIOBOT_H

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <tf/transform_broadcaster.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Temperature.h>

#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Char.h>
#include <visualization_msgs/MarkerArray.h>

#include "sunray_viobot/algo_ctrl.h"
#include "sunray_viobot/algo_status.h"
#include "sunray_viobot/viobot_ctrl.h"
#include "sunray_viobot/viobot_state.h"

#include "printf_utils.h"
#include "math_utils.h"

using namespace std;

class VioBot
{

public:
        // 构造函数
        VioBot(){};

        // 析构函数
        ~VioBot(){};

        // 初始化函数
        void init(ros::NodeHandle& nh, bool if_pritf);

        // 启动stereo2算法
        bool start_stereo2();

        // 关闭stereo2算法
        void shutdown_stereo2();

        sunray_viobot::algo_status algo_status;         // viobot算法状态

        nav_msgs::Odometry odom_rect;            // 里程计数据
        sunray_viobot::viobot_state viobot_state;       // viobot状态集合

        Eigen::Quaterniond q_viobot;             // 四元数
        Eigen::Vector3d euler_viobot;            // 欧拉角
        Eigen::Vector3d viobot_pos;              // 位置
        Eigen::Vector3d viobot_vel;              // 速度
        double viobot_yaw;                       // 偏航角  

        geometry_msgs::Quaternion q_viobot_msg;  // 四元数

        sensor_msgs::PointCloud2 rdf_point;
        sensor_msgs::PointCloud2ConstPtr rdf_point_ptr;

private:
        string node_name;

        sunray_viobot::algo_ctrl stereo2_ctrl;       // 启动stereo2算法      

        // 【发布】vio state
        ros::Publisher viobot_state_pub;
        // 【发布】stereo2算法启动、关闭、重置操作 to /viobot节点
        ros::Publisher pub_stereo2_ctrl;
        // 【订阅】算法状态 from /viobot节点
        ros::Subscriber viobot_algo_status_sub;
        // 【订阅】算法输出的里程计信息 from /viobot节点
        ros::Subscriber stereo2_odometry_rect_sub;

        void viobot_algo_status_cb(const sunray_viobot::algo_status::ConstPtr &msg);
        void stereo2_odometry_rect_cb(const nav_msgs::Odometry::ConstPtr &msg);

        Eigen::Vector3d quaternion_to_euler_viobot(const Eigen::Quaterniond &q);

};

#endif
